package com.fish;
import com.math.Quat;
import com.math.Vector;
/**
* @author Dror
*
* email: gumjum.o.o@gmail.com
*
*/
public class State {
Fish fi;
Vector tmp = new Vector();
Vector []p_p;
Quat []p_r;
static Vector foo = new Vector();
Vector p;
public Quat r;
Vector linVel;
Vector angVel;
Vector target;
Vector []tmpa;
static final int PREDICT_NUM = 9*3;
short [] tried = new short[PREDICT_NUM];
//Constants
static final float ANGULAR_ACC_X = (0.0174532925f*10);
static final float ANGULAR_ACC_Y = (0.0174532925f*10);
static final float ANGULAR_ACC_Z = (0.0174532925f*10);
// static final float ANGULAR_ACC_W = (float) (Math.PI/100f);
static final float ANGULAR_STOP_ACC_X = (ANGULAR_ACC_X*3f);
static final float ANGULAR_STOP_ACC_Y = (ANGULAR_ACC_Y*3f);
static final float ANGULAR_STOP_ACC_Z = (ANGULAR_ACC_Z*3f);
// static final float ANGULAR_STOP_ACC_W = (ANGULAR_ACC_Z*100f);
static final float MAX_ANG_VEL = +(0.0174532925f*20);
static final float MIN_ANG_VEL = -(0.0174532925f*20);
// static final float LIN_ACC = 0.3f;
// static final float LIN_STOP_ACC = 0.01f;
// static final float MAX_LIN_VEL = 2f;
// static final float MIN_LIN_VEL = MAX_LIN_VEL/5f;
public State(){
p = new Vector();
r = new Quat();
linVel = new Vector();
angVel = new Vector();
target = new Vector();
build();
}
public State(Fish f) {
p = new Vector(0,0,f.d.z);
p.transformation(f.r, f.p);
r = new Quat(f.r);
linVel = new Vector();
angVel = new Vector();
target = new Vector(f.target);
fi = f;
build();
}
public void set(State s){
p.set(s.p);
r.set(s.r);
linVel.set(s.linVel);
angVel.set(s.angVel);
target.set(s.target);
for(short r = 0;r<PREDICT_NUM;r++)
tried[r] = s.tried[r];
}
public void build(){
tmpa = new Vector[PREDICT_NUM];
resetTries();
p_p = new Vector[PREDICT_NUM];
p_r = new Quat[PREDICT_NUM];
for(int i=0;i<PREDICT_NUM;i++){
p_p[i] = new Vector();
p_r[i] = new Quat();
}
for(int i=0;i<PREDICT_NUM;i++)
tmpa[i] = new Vector();
}
public void computeAnAcc(Vector s, short i, int factor){
if(i == 0)
s.set(0 , 0 , 0);
if(i == 1)
s.set(+ANGULAR_ACC_X, 0+ANGULAR_ACC_Y, 0);
if(i == 2)
s.set(+ANGULAR_ACC_X, 0 , 0);
if(i == 3)
s.set(+ANGULAR_ACC_X, 0-ANGULAR_ACC_Y, 0);
if(i == 4)
s.set(0 , ANGULAR_ACC_Y, 0);
if(i == 5)
s.set(0 , -ANGULAR_ACC_Y, 0);
if(i == 6)
s.set(-ANGULAR_ACC_X, +ANGULAR_ACC_Y, 0);
if(i == 7)
s.set(-ANGULAR_ACC_X, 0 , 0);
if(i == 8)
s.set(-ANGULAR_ACC_X, -ANGULAR_ACC_Y, 0);
s.setScale(factor);
}
public void computeLiAcc(Vector s, short i,int factor){
if(i == 0)
s.set(0, 0, fi.speed);
if(i == 1)
s.set(0, 0, fi.speed*0.8f);
if(i == 2)
s.set(0, 0, fi.speed*0.5f);
s.setScale(factor);
}
public float predict(State s){
return predict(s,1);
}
public float predict(State s, int factor){
float [] predict = new float[PREDICT_NUM];
for(int i = 0;i< PREDICT_NUM;i++){
if(tried[i] == 1)
predict[i] = -1;
else
predict[i] = predictIt(i,factor);
}
int p = 0;
for(int i = 0;i<PREDICT_NUM;i++)
if(predict[i] < predict[p] && predict[i] != -1)
p = i;
setPredict(s,p,factor);
if(predict[p] != -1)
tried[p] = 1;
return predict[p];
}
public float fpredict(State s, int factor){
float [] predict = new float[PREDICT_NUM];
for(int i = 0;i< PREDICT_NUM;i++){
predict[i] = fpredictIt(i,factor,factor);
}
int p = 0;
for(int i = 0;i<PREDICT_NUM;i++)
if(predict[i] < predict[p] && predict[i] != -1)
p = i;
setPredict(s,p,factor);
if(predict[p] != -1)
tried[p] = 1;
return predict[p];
}
public float predictIt(int i){
return predictIt(i,1);
}
public float spePredict(State s, int factor,int linfactor, int i){
float tmp = predictIt(i,factor,linfactor);
setPredict(s,i,factor,linfactor);
return tmp;
}
public float predictIt(int i, int factor){
return predictIt(i,factor,factor);
}
public float predictIt(int i, int factor,int linfactor){
this.computeAnAcc(tmp, (short)(i%9),factor);
p_r[i].fromEulerAngles(tmp.x, tmp.y, 0);
p_r[i].multiply(r);
this.computeLiAcc(tmp, (short)(i/9),linfactor);
p_p[i].set(tmp);
p_p[i].rotate(p_r[i]);
p_p[i].x += p.x;
p_p[i].y += p.y;
p_p[i].z += p.z;
if(!World.checkFishPos(p_p[i],fi))
return -1;
tmp.set(target);
tmp.setSub(p_p[i]);
return tmp.length();
}
public float fpredictIt(int i, int factor,int linfactor){
this.computeAnAcc(tmp, (short)(i%9),factor);
p_r[i].fromEulerAngles(tmp.x, tmp.y, 0);
p_r[i].multiply(r);
this.computeLiAcc(tmp, (short)(i/9),linfactor);
p_p[i].set(tmp);
p_p[i].rotate(p_r[i]);
p_p[i].x += p.x;
p_p[i].y += p.y;
p_p[i].z += p.z;
// if(!world.checkFishPos(p_p[i],fi))
// return -1;
tmp.set(target);
tmp.setSub(p_p[i]);
return tmp.length();
}
private void setPredict(State s,int i,int factor) {
setPredict(s,i,factor,factor);
}
private void setPredict(State s,int i,int factor,int linfactor) {
s.resetTries();
s.p.set(p_p[i]);
s.r.set(p_r[i]);
this.computeLiAcc(tmp, (short)(i/9),linfactor);
s.linVel.set(tmp);
this.computeAnAcc(tmp, (short)(i%9),factor);
s.angVel.set(tmp);
s.target.set(target);
}
public void resetTries(){
for(short r = 0;r < PREDICT_NUM; r++)
tried[r] = 0;
}
public void setTarget(Vector target2) {
this.target.set(target2);
}
}